#include "CMatrix2d.h"




CMatrix2d::CMatrix2d()
{
	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = 0; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix2d::CMatrix2d( const fixed* m )
{ 
	ASSERT( m ); 

	MEMCPY( this, m, sizeof( CMatrix2d ) );

	ASSERT( !m_orientationFixupIsApplicable || 
			( m_orientationFixupIsApplicable && m_nOfArthmeticOpsTriggeringOrientationFixup ) );
}

 
CMatrix2d::CMatrix2d( fixed m00, fixed m01, fixed m10, fixed m11,
					  int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0.m_i = m00; 
	m_col0.m_j = m01;
	m_col1.m_i = m10; 
	m_col1.m_j = m11;

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup;

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix2d::CMatrix2d( const CVector2d* col, int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0 = *col; 
	m_col1 = *( col + 1 ); 

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix2d::CMatrix2d( const CVector2d& col0, const CVector2d& col1,
					  int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0 = col0; 
	m_col1 = col1; 

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix2d& CMatrix2d::Clear()
{
	m_col0.Clear(); 
	m_col1.Clear();

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}
 

CMatrix2d& CMatrix2d::Scale( fixed si, fixed sj )
{
	m_col0 *= si;
	m_col1 *= sj;

	m_orientationFixupIsApplicable = FALSE;

	return *this;
}


CMatrix2d& CMatrix2d::Scale( const CVector2d& s )
{
	m_col0 *= s.m_i;
	m_col1 *= s.m_j;

	m_orientationFixupIsApplicable = FALSE;

	return *this;
}


CMatrix2d& CMatrix2d::LoadIdentity()
{
	m_col0.m_i = CMathFixed::One;	m_col1.m_i = 0;
	m_col0.m_j = 0;					m_col1.m_j = CMathFixed::One;

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix2d& CMatrix2d::Orthonormalize()
{
	CVector2d b0( m_col0.m_i, m_col1.m_i );
	CVector2d b1( m_col0.m_j, m_col1.m_j );

	b0.Normalize();

	fixed dOrtho = b0 * b1;

	if( dOrtho )
	{
		b1.m_i -= CMathFixed::Mul( b0.m_i, dOrtho );
		b1.m_j -= CMathFixed::Mul( b0.m_j, dOrtho );
	}
		
	b1.Normalize();
	
	m_col0.Set( b0.m_i, b1.m_i );
	m_col1.Set( b0.m_j, b1.m_j );

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix2d& CMatrix2d::Rotate( fixed degrees )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];

	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );

	a[0] = CMathFixed::Mul( a0, cos ) + CMathFixed::Mul( a[2], sin );
	a[1] = CMathFixed::Mul( a1, cos ) + CMathFixed::Mul( a[3], sin );

	a[2] = CMathFixed::Mul( a0, -sin ) + CMathFixed::Mul( a[2], cos );
	a[3] = CMathFixed::Mul( a1, -sin ) + CMathFixed::Mul( a[3], cos );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix2d& CMatrix2d::operator*=( const CMatrix2d& b )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	
	a[0] = CMathFixed::Mul( a0, b[0] ) + CMathFixed::Mul( a[2], b[1] );
	a[1] = CMathFixed::Mul( a1, b[0] ) + CMathFixed::Mul( a[3], b[1] );

	a[2] = CMathFixed::Mul( a0, b[2] ) + CMathFixed::Mul( a[2], b[2] );
	a[3] = CMathFixed::Mul( a1, b[3] ) + CMathFixed::Mul( a[3], b[3] );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix2d operator*( const CMatrix2d& a, const CMatrix2d& b )
{
	return CMatrix2d( CMathFixed::Mul( a[0], b[0] ) + CMathFixed::Mul( a[2], b[1] ),
					  CMathFixed::Mul( a[1], b[0] ) + CMathFixed::Mul( a[3], b[1] ),

					  CMathFixed::Mul( a[0], b[2] ) + CMathFixed::Mul( a[2], b[2] ),
					  CMathFixed::Mul( a[1], b[3] ) + CMathFixed::Mul( a[3], b[3] ),
					  
					  a.GetNumOfArithmeticOpsTriggeringOrientationFixup() );
}


inline
void CMatrix2d::IncrementOperationCount()
{
	if( m_nOfArithmeticOpsSinceLastFixup < MAX_UINT16 )
		m_nOfArithmeticOpsSinceLastFixup++;
}


inline
void CMatrix2d::ConsiderOrientationFixup()
{
	if( m_orientationFixupIsApplicable &&
		m_nOfArithmeticOpsSinceLastFixup >= m_nOfArthmeticOpsTriggeringOrientationFixup )
		Orthonormalize();
}
